﻿namespace RLab.Visualization

open System
open System.Drawing
open System.Windows.Forms
open Microsoft.DirectX
open Microsoft.DirectX.Direct3D

open GeometryDefinitions
open RLab.Programming

type PhysicScara(r,e,chain,toolBuilder)= 
        inherit  PhysicRobot(r,e,chain,toolBuilder)
        let event = Event<ProgrammableEventArgs>()        
        let logAction msg = 
            event.Trigger( Log(1,msg) )
        interface IProgrammable with   
//todo:refactor, Apply OOP instead of matches         
            member s.Base() =                 
                logAction "Moving to base position"
                base.Move("ARM2*HAND",0.0f,30.f)
                base.Move("BASE*ARM1",0.0f,PhysicConstants.defaultAngleVelocity/4.0f)
                base.Move("ARM1*ARM2",0.0f,PhysicConstants.defaultAngleVelocity/4.0f)               
                base.Move("HAND*TOOLROLL",0.0f,PhysicConstants.defaultAngleVelocity/4.0f)
            member s.Drive joint movement veloPercent = 
                let converted = 
                    match joint with
                    | 1 | 2 | 4 -> -2.0f*pi*movement/360.0f
                    | _ -> -movement                          
                logAction (String.Format("Moving joint {0} to {1}",joint,movement))
                let wsk = 3.0f//wskaznik predkosci
                let jointID,velocity = 
                    match joint with
                    | 1 -> "BASE*ARM1",PhysicConstants.defaultAngleVelocity/wsk
                    | 2 -> "ARM1*ARM2",PhysicConstants.defaultAngleVelocity/wsk
                    | 3 -> "ARM2*HAND",120.f/wsk
                    | 4 -> "HAND*TOOLROLL", PhysicConstants.defaultAngleVelocity/wsk
                    | _ -> failwith "joint not exists"

                base.Move(jointID,converted,velocity*veloPercent/100.f)
            member s.OpenTool() = 
                logAction "Opening tool" 
                base.Tool.Open()
            member s.CloseTool() = 
                logAction "Closing tool"
                base.Tool.Close()
            member s.Type message = 
                event.Trigger( Console( message))
            member s.TraceEvent = 
                event.Publish

[<CompilationRepresentation(CompilationRepresentationFlags.ModuleSuffix)>]
module Scara = 
    let CreatePhysicScara ( r: Renderer)  (e : PhysicEngine) = 
        let boxv = V3(150.f,700.f,150.f)
        let rbase = 
            { Name = "BASE"
              Model = r.LoadMeshFromFile(@"base.x")
              PhysicModel = boxv, V3(0.f,boxv.Y/2.f,0.f)
              Mass = 10.f
              CenterOfMassOffset = V3(0.f,-boxv.Y/2.f,0.f) }

        let boxArm = V3(425.0f*2.f, 120.0f,150.f)
        let arm1 = 
            { Name = "ARM1" 
              Model = 
                //r.CreateMeshBox(boxArm) 
                r.LoadMeshFromFile(@"element1.x")
              PhysicModel = boxArm,V3(0.f,boxv.Y+boxArm.Y/2.f,0.f)
              Mass = 2.f
              CenterOfMassOffset =
                //O
                V3(0.f,-boxArm.Y/2.f,0.f) 
              }
        let baseArmJoint = 
            { Name = "BASE*ARM1"
              Type = Revolute
              Anchor = snd arm1.PhysicModel
              Axis = snd arm1.PhysicModel }

        let boxArm2 = new Vector3( 350.0f,100.0f, 120.0f )            
        let arm2GlobalPose = V3(boxArm.X/2.f+boxArm2.X/2.0f,boxv.Y+boxArm.Y+boxArm2.Y/2.f,0.f)
        let arm2 = 
            { Name = "ARM2"
              Model = 
                //r.CreateMeshBox(boxArm2)
                r.LoadMeshFromFile(@"element2.x")
              PhysicModel = boxArm2,arm2GlobalPose
              Mass = 0.3f
              CenterOfMassOffset =
                //O
                V3(-boxArm2.X/2.f,-boxArm2.Y/2.f,0.f) 
              }
        let arm1arm2Joint = 
            { Name = "ARM1*ARM2"
              Type = Revolute
              Anchor = V3(boxArm.X/2.f,arm2GlobalPose.Y,arm2GlobalPose.Z)
              Axis = V3( 0.f,10.f,0.f) } 

        let handBox = V3(50.f,300.f,50.f)
        let handposoffset = 0.f
        let handGlobalPose = V3( arm2GlobalPose.X + boxArm2.X/2.f + handBox.X/2.f,arm2GlobalPose.Y - 
                                handposoffset
                                ,arm2GlobalPose.Z)
        let hand =
            { Name = "HAND"
              Model =                 
                r.LoadMeshFromFile(@"hand.x")
              PhysicModel = handBox,handGlobalPose 
              Mass = 0.2f
              CenterOfMassOffset = 
                //O
                V3(0.f,-handBox.Y/2.f,0.f)
            }
        let arm2HandJoint = 
            { Name = "ARM2*HAND"
              Type = Prismatic(VelocityDrive)
              Anchor = handGlobalPose + V3(0.f,handposoffset,0.f)
              Axis = V3( 0.f,10.f,0.f) }
        
        let toolRollBox = V3(25.0f,5.f,70.f)
        let toolRollGlobalPose = V3(handGlobalPose.X, 
                                    handGlobalPose.Y - toolRollBox.Y/2.f - handBox.Y/2.0f,
                                    handGlobalPose.Z)

        let toolRoll = 
            { Name = "TOOLROLL"
              Model = r.CreateMeshBox(toolRollBox)
              PhysicModel = toolRollBox,toolRollGlobalPose
              Mass = 0.05f
              CenterOfMassOffset = V3(0.f,-toolRollBox.Y/2.f,0.f) }
        
        let handRollJoint = 
            { Name = "HAND*TOOLROLL"
              Type = Revolute
              Anchor = toolRollGlobalPose
              Axis = V3( 0.f,10.f,0.f) } 
        

        let chain = Chain(rbase,
                        Link(baseArmJoint,arm1,
                            Link(arm1arm2Joint,arm2,
                                Link(arm2HandJoint,hand,
                                    Link(handRollJoint,toolRoll,End)))))
        new PhysicScara(r,e,chain,(fun()->Tool.hand r e))